#ifndef DRIVETRAIN_H
#define DRIVETRAIN_H

#include "Commands/Subsystem.h"
#include "WPILib.h"
#include "../Commands/TeleOpCommand.h"
#include <math.h>

/**
 *
 *
 * @author team308
 */
class Drivetrain: public Subsystem {
private:
	
	// It's desirable that everything possible under private except
	// for methods that implement subsystem capabilities
	// set the P, I, and D constants here
	static const float kPLeft = 0.0;
	static const float kILeft = 0.0;
	static const float kDLeft = 0.0;
	static const float kPRight = 0.0;
	static const float kIRight = 0.0;
	static const float kDRight = 0.0;
	
	static const float kWheelSpeed = 20.0; // Put max ft/s of slowest wheel
	
	static const float kLDistanceThresh = 1.0;
	static const float kRDistanceThresh = 1.0; //In feet?
public:	
	bool distanceDriving;
	
	Jaguar leftJag;
	Jaguar rightJag;
	Encoder leftEncoder;
	Encoder rightEncoder;
	PIDController leftPid;
	PIDController rightPid;
	RobotDrive robotDrive;
	//PIDController rightPID;
	//PIDController leftPID;
	void CheckDistanceDrive();
	void DriveDistance(float dL, float dR);
	void DriveRobot(float X, float Y);
	float Scale(float value, float min, float max, float newmin,
			float newmax); 
	float DeadZone(float input, float dz);
	//void DriveWithPID(float SpeedLeft,float SpeedRight);
	//void MonstersCustomPIDArcadeDriveExtravaganza(float x , float y, float throttle);
	//float SpeedNormalization(float Left , float Right);
	Drivetrain();
	void InitDefaultCommand();
};

#endif
